// PENDELNUMMER: B001129
// Das pendel muss anfangs so kalibiert werden, dass die variable winkel im main
// in der ruheposition des Pendels (nach unten hängend) den wert 0 hat und im Gegenuhrzeiger
// Sinn zunehmend bis 4096 steigt. (4096 = 1 volle umdrehung). D.h. ganz oben (balanced position = 2048)
// Dazu einfach das printf im main auskommentieren und mittels serial window die Pendelpositon ablesen und im
// File controller.c  den wert: a0_offset = XXX; setzten.

// Aufschwingvideo unter: http://thomasreinbacher.wasner.it

// Wünsche, Fragen, oä:
// Josef Maier		es07m104@technikum-wien.at
// Thomas Reinbacher	es07m106@technikum-wien.at
// Ch. Schönegger	es07m107@technikum-wien.at

#include <reg167.h>
#include <stdio.h>

//#include "sim_terminal.h"
#include "stepper.h"
#include "rs232.h"
#include "encoder.h"
#include "time.h"
#include "controller.h"
#include "globals.h"
#include "config.h"
#include "opt_swingup.h"
#include <math.h>

// Defines ----------------------------------------------------------

#define OUTPUT_ENABLE 1

/* defines for rocking chair algorythm */
#define SWUP_ROCKING_CHAIR_STEPSIZE (115)

// Function Prototypes ----------------------------------------------------------
void swing(int steps, int direction);
void wait(unsigned int t);
int divFactorToBitPos(int divFactor);

extern float swup_x2_iso;

// Functions -------------------------------------------------------------------

void main (void) 
{
    char c;
    
    int winkel;
    int winkel_old = 0;
    
    const int  FACT_FULL = 4096;
    const int  FACT_16 = 256;
    
    int divFactor = 0x00;
    

    
    time_init();	  			// muss als erstes kommen weil RS232 und stepper darauf aufbauen
    send_receive_init();	    // Soft RS232 initialisieren
    stepper_init();			// Schrittmotor Treiber initialisieren
    init_SSI();				// Init Synchron Serial Interface
    
    //a0_offset = read_rotaryencoder(1);
    a0_offset = 0x840;

    while((read_rotaryencoder(1)>(a0_offset-2048)) && (read_rotaryencoder(1)<(a0_offset+2048)));

    controller_init();	// ISR fuer Regler initialisieren
    
    T5R=1;
    
    IEN=1;
    
    startRegler = 0x00;
    Wx0_iso_Sollpos = 0;
    
    printf("\n");


    while (1)
    {
        winkel = a0 + 2048;  

        //printf("Winkel = %d \r ", winkel);
        //printf("%f; %f; %d \r ", E_kin_iso,E_pot_iso,winkel);
        //printf("%f; %f; %d \r", x2_iso, swup_x2_iso, winkel);
        //printf("%f \r", x0_iso);
        printf("%f; %f; %f; %f; %f \r", swup_x2_iso, a0_iso, a1_iso, E_kin_iso, E_pot_iso); 
        
        /** Wenn im Aufrechten Bereich, dann Starte den HauptRegler */
        if(((winkel > ((FACT_FULL/2)) - 75)&&(winkel < ((FACT_FULL/2)) + 75))&&(startRegler==0x00))
        {
            x0 = 0x00;
        	startRegler = 0x01;
         	Wx0_iso_Sollpos = 0;
        }
    						  
    	/** Wenn wieder umgefallen, dann starte Auschwingregler */
    	if(((winkel < (1024))||(winkel > (3072))) && (startRegler!=0x00))
    	{
    	 	startRegler = 0x00;					  
    	}
	 	 	
        /** Solange der Hauptregler nicht gestartet wurde, versuche aufzuschwingen */
        if(startRegler == 0x00)
        {
            
            #if (SWUP_ALGO==SWUP_ROCKING_CHAIR)
            {
            
                if((winkel > 256)&&(winkel < (512)) && (winkel_old > winkel))
                {
                    /** Fahre nach Rechts */
                    if(OUTPUT_ENABLE) 
                    {
                        Wx0_iso_Sollpos = SWUP_ROCKING_CHAIR_STEPSIZE;
                    }
                }
                else if((winkel < 3840)&&(winkel > (3584)) && (winkel_old < winkel))
                {			  			
                    /** Fahre nach Links */
                    if(OUTPUT_ENABLE) 
                    {
                        Wx0_iso_Sollpos = -(SWUP_ROCKING_CHAIR_STEPSIZE);
                    }
                }
            }
            #endif /* (SWUP_ALGO==SWUP_ROCKING_CHAIR) */
            
            #if (SWUP_ALGO==SWUP_ENERGY_MEAS)
            {

            }
            #endif /* (SWUP_ALGO==SWUP_ENERGY_MEAS) */
            
            winkel_old = winkel;
    	}	
    	else
    	{	
    	 	if (testkey ()) 
    	 	{
    			c=_getkey();
    			switch (c) 
    			{
    				case 'd': Wx0_iso_Sollpos = Wx0_iso_Sollpos + 300;  printf("Sollpos == %4.1f ", Wx0_iso_Sollpos ); break;
    				case 'a': Wx0_iso_Sollpos = Wx0_iso_Sollpos - 300;  printf("Sollpos == %4.1f ", Wx0_iso_Sollpos ); break;
    				case 's': startRegler = 0xFF; x2=0; x1=0; break;
    				default: break;
    			}
    		}
    	}
    } 
} 
 
int divFactorToBitPos(int divFactor)
{
    switch(divFactor)
    {
        case 1: return 0;
        case 2: return 1;
        case 4: return 2;
        case 8: return 3;
        case 16: return 4;
        case 32: return 5;
        case 64: return 6;
        case 128: return 7;
        case 256: return 8;
        case 512: return 9;
        default: return 0;
    }
}

void wait(unsigned int t)
{
  int u=0x00;
	for(u=0x0; u< t; u++);
}